#include "ModBusSlave0.h"


#define TRANSMIT_PIN 6
#define MODBUS_ADDRESS 100

ModBusSlave0 mod(false);

ISR(USART0_RX_vect)
{
  mod.receiveHandler();
}

ISR(USART0_TX_vect)
{
  mod.transmitHandler();
}

uint16_t get_addr(uint16_t addr){
  switch(addr){
    case 1:
      return 1;
    default:
      return addr;
  }
}

uint8_t prepareResponse03(uint8_t *frame, uint8_t* data) {
  if(frame[1] == 0x03){
    uint16_t addr = uint16_t(frame[2])*256 + uint16_t(frame[3]);
    uint16_t len = uint16_t(frame[4])*256 + uint16_t(frame[5]);

    int i = 0;
    while(len --){
      data[i] = (get_addr(addr + i/2)&0xff00)>>8;
      data[i + 1] = get_addr(addr + i/2)&0x00ff;
      i +=2 ;
    }
    return i;
  }
  if(frame[1] == 0x06){
    
  }

}

void setup() {
  mod.begin(57600, TRANSMIT_PIN, MODBUS_ADDRESS);
  mod.prepareResponse03 = *prepareResponse03;
}

void loop() {
  mod.process();
}

